/**
* File: search.cpp
*
* \brief Pathfinding structures and algorithms.
*
* \author Instituto Superior Tecnico de Lisboa, Portugal
* \author Technical University of Lisbon, Portugal
* 
* \author Agentes Autonomos e Sistemas Multiagente
* \author Autonomous Agents and Multiagent Systems
* 
* \version	1.0
* \date		2006/2007
**/
#include "search.h"
#include "waypoint.h"
using namespace game::search;
using namespace game::waypoints;
#include "../util/util.h"
using namespace util;
#include "../collections/QueuePriority.h"
using namespace collections;

int *game::search::g_pFloydDistanceMatrix = 0;
int *game::search::g_pFloydPathMatrix = 0;

/**
* \brief Initialize the distance matrices for the Floyd-Warshall algorithm. 
* 
* Should be invoked before calling 'game::search::floydWarshallShortestPath'.
**/
void game::search::initializeFloydWarshallMatrices()
{
	int i, j, k;

	if (g_pFloydDistanceMatrix != NULL)
	{
		free(g_pFloydDistanceMatrix);
		g_pFloydDistanceMatrix = NULL;
	}

	if (g_pFloydPathMatrix != NULL)
	{
		free(g_pFloydPathMatrix);
		g_pFloydPathMatrix = NULL;
	}

	g_pFloydDistanceMatrix = (int *)calloc(g_numWaypoints * g_numWaypoints, sizeof(int));
	g_pFloydPathMatrix = (int *)calloc(g_numWaypoints * g_numWaypoints, sizeof(int));

	if (g_pFloydDistanceMatrix == NULL || g_pFloydPathMatrix == NULL)
		util::terminateOnError("Memory Allocation Error!");

	for (i = 0; i < g_numWaypoints; i++)
	{
		for (j = 0; j < g_numWaypoints; j++)
		{
			*(g_pFloydDistanceMatrix + i * g_numWaypoints + j) = 999999;
			*(g_pFloydPathMatrix + i * g_numWaypoints + j) = -1;
		}
	}

	for (i = 0; i < g_numWaypoints; i++)
	{
		for (j = 0; j < MAX_PATH_INDEX; j++)
		{
			if (g_waypoints[i]->index[j] >= 0 && g_waypoints[i]->index[j] < g_numWaypoints)
			{
				*(g_pFloydDistanceMatrix + i * g_numWaypoints + g_waypoints[i]->index[j]) = g_waypoints[i]->distance[j];
				*(g_pFloydPathMatrix + i * g_numWaypoints + g_waypoints[i]->index[j]) = g_waypoints[i]->index[j];
			}
		}
	}

	for (i = 0; i < g_numWaypoints; i++)
		*(g_pFloydDistanceMatrix + i * g_numWaypoints + i) = 0;

	for (k = 0; k < g_numWaypoints; k++)
	{
		for (i = 0; i < g_numWaypoints; i++)
		{
			for (j = 0; j < g_numWaypoints; j++)
			{
				if (*(g_pFloydDistanceMatrix + i * g_numWaypoints + k) + *(g_pFloydDistanceMatrix + k * g_numWaypoints + j)
					<(*(g_pFloydDistanceMatrix + i * g_numWaypoints + j)) )
				{
					*(g_pFloydDistanceMatrix + i * g_numWaypoints + j) = *(g_pFloydDistanceMatrix + i * g_numWaypoints + k)
						+ *(g_pFloydDistanceMatrix + k * g_numWaypoints + j);
					*(g_pFloydPathMatrix + i * g_numWaypoints + j) = *(g_pFloydPathMatrix + i * g_numWaypoints + k);
				}
			}
		}
	}
}

/**
* \brief Cost function (g) for a search method. Cost is based on distance between 
* waypoint and its parent.
*
* \param[in] index The waypoint index.
* \param[in] parentIndex The waypoint's parent index.
**/
float game::search::costFunctionPathDistance(int index, int parentIndex)
{
	if (parentIndex == -1)
		return 0;

	for (int i = 0; i < MAX_PATH_INDEX; i++)
	{
		if (g_waypoints[parentIndex]->index[i] == index)
		{
			if (g_waypoints[index]->flags & (WaypointFlags::ladder | WaypointFlags::crouch))
				return g_waypoints[parentIndex]->distance[i] * 1.5; // Ladder or crouch waypoints cost more
			return g_waypoints[parentIndex]->distance[i];
		}
	}

	return 65355; //No path between waypoint and its parent
}

/**
* \brief Cost function (g) for a search method which avoids 'no hostage' waypoints. 
*
* Cost is based on distance between waypoint and its parent.
*
* \param[in] index The waypoint index.
* \param[in] parentIndex The waypoint's parent index.
**/
float game::search::costFunctionPathDistanceAvoidNoHostage(int index, int parentIndex)
{
	if (g_waypoints[index]->flags & WaypointFlags::noHostage)
		return 65355; //maximum cost
	else if (g_waypoints[index]->flags & (WaypointFlags::crouch | WaypointFlags::ladder))
		return game::search::costFunctionPathDistance(index, parentIndex) * 500;
	return game::search::costFunctionPathDistance(index, parentIndex);
}

/**
* \brief Heuristic function (h) for an informed search method. 
*
* The heuristic adds the absolute values of the differences of the components 
* of the goal origin with the waypoint origin
*
* \param[in] index The waypoint index.
* \param[in] startIndex The initial waypoint index.
* \param[in] goalIndex The goal waypoint index.
**/
float game::search::heuristicFunctionPathDistance(int index, int startIndex, int goalIndex)
{
	float h = fabs(g_waypoints[index]->origin.x - g_waypoints[goalIndex]->origin.x);
	h += fabs(g_waypoints[index]->origin.y - g_waypoints[goalIndex]->origin.y);
	h += fabs(g_waypoints[index]->origin.z - g_waypoints[goalIndex]->origin.z);

	return h;
}


/**
* \brief Finds a path between two waypoints.
*
* \param[in] startIndex The source waypoint index.
* \param[in] endIndex The destination waypoint index.
* \param[in] costFunction The cost function to use - g(n).
* \param[in] heuristicFunction The heuristic function to use - h(n).
**/
PathNode* game::search::aStar(
				 int startIndex, int endIndex, 
				 float (*costFunction)(int, int), 
				 float (*heuristicFunction)(int, int, int))
{
	if (startIndex > g_numWaypoints - 1 || startIndex < 0)
		return NULL; //Invalid source

	if (endIndex > g_numWaypoints - 1 || endIndex < 0)
		return NULL; //Invalid destination

	struct AStarList astar_list[MAX_WAYPOINTS];

	QueuePriority openList(MAX_STACK_NODES);

	int i;
	for (i = 0; i < MAX_WAYPOINTS; i++)
	{
		astar_list[i].g = 0;
		astar_list[i].f = 0;
		astar_list[i].iParentIndex = -1;
		astar_list[i].iState = NEW;
	}

	// put start node into open list
	astar_list[startIndex].g = costFunction(startIndex, -1);
	astar_list[startIndex].f = astar_list[startIndex].g + heuristicFunction(startIndex, startIndex, endIndex);
	astar_list[startIndex].iState = OPEN;

	openList.Insert(startIndex, astar_list[startIndex].g);

	PathNode *rootNode;
	while (!openList.Empty())
	{
		// remove the first node from the open list
		int iCurrentIndex = openList.Remove();

		// is the current node the goal node?
		if (iCurrentIndex == endIndex)
		{
			// build the complete path
			rootNode = NULL;
			do
			{
				PathNode *pathNode = (PathNode *)malloc(sizeof(PathNode));
				if (pathNode == NULL)
					return false;

				pathNode->iIndex = iCurrentIndex;
				pathNode->NextNode = rootNode;
				rootNode = pathNode;
				iCurrentIndex = astar_list[iCurrentIndex].iParentIndex;
			} while (iCurrentIndex != -1);

			return rootNode;
		}

		if (astar_list[iCurrentIndex].iState != OPEN)
			continue;

		// put current node into CLOSED list
		astar_list[iCurrentIndex].iState = CLOSED;

		// now expand the current node
		for (i = 0; i < MAX_PATH_INDEX; i++)
		{
			int iCurChild = g_waypoints[iCurrentIndex]->index[i];
			if (iCurChild == -1)
				continue;

			// calculate the f value as f = g + h
			float g = astar_list[iCurrentIndex].g + costFunction(iCurChild, iCurrentIndex);
			float h = heuristicFunction(iCurChild, startIndex, endIndex);
			float f = g + h;

			if (astar_list[iCurChild].iState == NEW ||
				astar_list[iCurChild].f > f)
			{
				// put the current child into open list
				astar_list[iCurChild].iParentIndex = iCurrentIndex;
				astar_list[iCurChild].iState = OPEN;
				astar_list[iCurChild].g = g;
				astar_list[iCurChild].f = f;
				openList.Insert(iCurChild, g);
			}
		}
	}

	return NULL;
}
/**
* \brief Finds the shortest path from startIndex to endIndex using the 
* Floyd-Warshall shortest path algorithm. 
**/
PathNode* game::search::floydWarshallShortestPath(int startIndex, int endIndex)
{
	PathNode *node = (PathNode *)malloc(sizeof(PathNode));
	if (node == NULL)
		return NULL;

	PathNode *rootNode = node;
	node->iIndex = startIndex;
	node->NextNode = NULL;

	while (startIndex != endIndex)
	{
		startIndex = *(g_pFloydPathMatrix + startIndex * g_numWaypoints + endIndex);
		if (startIndex < 0)
			return NULL;
		node->NextNode = (PathNode *)malloc(sizeof(PathNode));
		node = node->NextNode;
		if (node == NULL)
			return NULL;
		node->iIndex = startIndex;
		node->NextNode = NULL;
	}

	return rootNode;
}